#include <Eigen/Core>
#include <opencv2/core.hpp>
#include <QDebug>

#include <common/utils/MeStrUtil.h>
#include <common/utils/MeStrUtil.h>

#include <memath/MeEigenMath.h>
#include <memath/matrixd.h>
#include <memath/meTransform.h>

#include <algorithm/transform/CirclesAxisCalbration.h>
#include <algorithm/transform/RigidCoordCalibration.h>

using namespace std;
using namespace cv;

/// 坐标旋转顺序
static void demoCoordRotSeq()
{
    // 设置坐标系A, B
    // 验证旋转和平移以后如何计算 转化矩阵
    // B坐标系平移t, 然后再旋转-30度最后的矩阵是 M_BfrA
    double t[3] = {0, 2, 0};
    Matrix4d m1; m1.identity();
    m1.translate(t);  // 平移
    Matrix4d m2 = MeTransform::rotateZ(meDegreesToRadians(-30)); // 旋转
    Matrix4d m_BfrA = m1*m2;
    cout << "m_BfrA:\n" << m_BfrA << endl;
    Eigen::Vector3d pt_A = {1, 0, 0};
    Eigen::Vector3d pt_B = MeEigenMath::multiplyVector3d(m_BfrA, pt_A);
    qDebug() << "pt_B: " << MeStrUtil::point3dToQStr(pt_B.data());
}


/// 矩阵转化关系
static void demoCoordMatrix()
{

    vector<Eigen::Vector3d> pts_B1, pts_B2, pts_B3, pts_B4;
    Matrix4d m_A0frA1, m_A1frA2, m_A1frA3, m_A1frA4;

    // 构造测试数据 pts_B1, pts_B2, m_A1frA2
    {
        // 两个坐标系 B, A
        vector<Eigen::Vector3d> pts_A1 = {
            {-9.310000, 46.840000, -6.840000},
            {-40.070000, 21.120000, 12.970000},
            {-10.480000, 36.540000, 48.690000},
            {-29.510000, 18.500000, 73.460000}
        };
//        double t2[3] = {10, 20, 30};
//        double t3[3] = {0, 0, 30};

//        m_A1frA2 = MeTransform::rotateZ(meDegreesToRadians(30)); //m_A1frA2.translate(t2);
//        m_A1frA3 = MeTransform::rotateY(meDegreesToRadians(30)); m_A1frA3.translate(t3);

        double t2[3] = {0, 0, 10};
        double t3[3] = {0, 10, 0};
        m_A0frA1  = MeTransform::rotateZ(meDegreesToRadians(0));
        m_A1frA2 = MeTransform::rotateZ(meDegreesToRadians(30)); m_A1frA2.translate(t2);
        //m_A1frA3 = MeTransform::rotateY(meDegreesToRadians(30)); m_A1frA3.translate(t3);
        m_A1frA3 = RigidCoordCalibration::calMatrixRotateJ5(meDegreesToRadians(30), 94);
        //Matrix4d m1 = MeTransform::rotateZ(meDegreesToRadians(-30));
        //m_A1frA3 = m1 * m_A1frA3 * m1.getInverseHomogeneous();
        //m_A1frA3.translate(t3);

        m_A1frA4 = RigidCoordCalibration::calMatrixRotateJ4(meDegreesToRadians(30), 102.5);

        Matrix4d m_BfrA1 = MeTransform::rotateZ(meDegreesToRadians(10));
        double t[3] = {10, 20, 30};
        m_BfrA1.translate(t);

        Matrix4d m_BfrA2 = m_BfrA1 * m_A1frA2;
        Matrix4d m_BfrA3 = m_BfrA1 * m_A1frA3;
        Matrix4d m_BfrA4 = m_BfrA1 * m_A1frA4;

        foreach (const Eigen::Vector3d &p, pts_A1) {
            pts_B1.push_back(MeEigenMath::multiplyVector3d(m_BfrA1, p));
            pts_B2.push_back(MeEigenMath::multiplyVector3d(m_BfrA2, p));
            pts_B3.push_back(MeEigenMath::multiplyVector3d(m_BfrA3, p));
            pts_B4.push_back(MeEigenMath::multiplyVector3d(m_BfrA4, p));
        }
        qDebug() << "pts_A1: " << MeStrUtil::pointsToQString(pts_A1);
    }

    vector<Matrix4d> m_AxfrAy = {
        m_A0frA1,
        m_A1frA2,
        m_A1frA3
    };
    vector<std::vector<Eigen::Vector3d>> points_C = {
        pts_B1,
        pts_B2,
        pts_B3
    };


    Matrix4d m_A1frB1;
    RigidCoordCalibration::calRigidMatrix_AfrB(m_AxfrAy,
                                       points_C, m_A1frB1);
    //RigidCoordCalibration::calRigidMatrix_AfrB(m_A1frA2, m_A1frA3, m_A1frA4, pts_B1, pts_B2, pts_B3, pts_B4, m_A1frB1);

    cout << "m_A1frB1:" << m_A1frB1;
    foreach (const Eigen::Vector3d &p, pts_B1) {
        Eigen::Vector3d p_A1 = MeEigenMath::multiplyVector3d(m_A1frB1, p);
        qDebug() << "pt_A: " << MeStrUtil::point3dToQStr(p_A1.data());
    }

}

///  dmeo CirclesAxisCalbration
static void demoCircleAxisCalib()
{
    double rA = 3;
    double rB = 5;
    std::vector<Eigen::Vector3d> pAs = {
        {3+rA*cos(0), rA*sin(0), -3},
        {3+rA*cos(45*M_PI/180), rA*sin(45*M_PI/180), -3},
        {3+rA*cos(90*M_PI/180), rA*sin(90*M_PI/180), -3.1},
        {3+rA*cos(190*M_PI/180), rA*sin(190*M_PI/180), -3},
    };
    std::vector<Eigen::Vector3d> pBs = {
        {3+rB*cos(0), rB*sin(0), 10},
        {3+rB*cos(45*M_PI/180), rB*sin(45*M_PI/180), 10},
        {3+rB*cos(90*M_PI/180), rB*sin(90*M_PI/180), 10},
        {3+rB*cos(190*M_PI/180), rB*sin(190*M_PI/180), 10},
    };

    Eigen::Vector3d cA, cB, normV;

    Eigen::Vector3d expectNorm{0, 0, 1};
    if (CirclesAxisCalbration::calibCircleCenter(pAs,cA)) {
        qDebug() << "centerA:" << MeStrUtil::point3dToQStr(cA.data());
    }
    if (CirclesAxisCalbration::calibCirclePlanesNorm(pAs,pBs, normV)) {
        double angle = MeMath::angleBetweenVectors(expectNorm.data(), normV.data());
        qDebug() << "normV:" << MeStrUtil::point3dToQStr(normV.data()) << "angle:" << angle;
    }
    /// calibCirclePlanesNorm2 计算的误差最小
    if (CirclesAxisCalbration::calibCirclePlanesNorm2(pAs,pBs, normV)) {
        double angle = MeMath::angleBetweenVectors(expectNorm.data(), normV.data());
        qDebug() << "calibCirclePlanesNorm2 normV:" << MeStrUtil::point3dToQStr(normV.data()) << "angle:" << (angle-M_PI);
    }
    if (CirclesAxisCalbration::calibPlanesNorm(pAs, normV)) {
        double angle = MeMath::angleBetweenVectors(expectNorm.data(), normV.data());
        qDebug() << "calibPlanesNorm normA:" << MeStrUtil::point3dToQStr(normV.data()) << "angle:" << angle;;
    }

    if (CirclesAxisCalbration::calibCirclesAxis(pAs, pBs, cA, cB)) {
        qDebug() << "centerA:" << MeStrUtil::point3dToQStr(cA.data()) << " centerB:"<< MeStrUtil::point3dToQStr(cB.data());
    }
}

int main(int argc, char *argv[])
{
    int option = 2;
    switch (option) {
    case 0:
        demoCoordRotSeq();
        break;
    case 1:
        demoCoordMatrix();
        break;
    case 2:
        demoCircleAxisCalib();
        break;
    case 99:
    {
        Matrix4d m_A1frA4 = RigidCoordCalibration::calMatrixRotateJ4(meDegreesToRadians(45), 102.5);
        cout << "m_A1frA4:\n" << m_A1frA4 << endl;
        Matrix4d m1 = RigidCoordCalibration::calMatrixRotateJ4(meDegreesToRadians(10), 102.5);
        Matrix4d m2 = RigidCoordCalibration::calMatrixRotateJ4(meDegreesToRadians(35), 102.5);
        Matrix4d m3 = m1*m2;
        cout << "m3:\n" << m3 << endl;
    }
        break;
    default:
        break;
    }
    return 0;
}
